

#include "errlog.h"
#include "grid3d.h"
#include "g3dutils.h"
#include "g3dqual.h"
#include <OptMS.h>           /* in opt-ms/include */
#include <stdlib.h>
#include <string.h>
// #include <time.h>

node_t *saved_nodes=NULL;
int *node_order=NULL;
double *Tqual=NULL, *nqual=NULL;
double minQ;
void *smooth_data=NULL;

static void qg_fill_Xqual(grid3d_t *grid, node_t *nodes);
static int _comp_node_order(const void *A, const void *B);
static void qg_local_connect(grid3d_t *grid, node_t *nodes, int nid,
    double **lFree, int *lNN, int *lNEL,
    double ***lCoords, int ***lConn);
const int max_passes=20;
const int min_passes=3;

void QGrid(grid3d_t *grid, node_t *nodes, int *can_move)
/* IMPORTANT NOTE: can_move is NOT a TRUE/FALSE array.
    A node can not be moved if its entry in can_move is negative */
{
    int i, j, nid, pass;
    double IQ, SQ;
    double tm=MPI_Wtime();

    if(node_order==NULL)
        init_g3dqual(grid);

    pass=0;
    for(i=0; i<grid->NN; i++)
        node_order[i]=i;

    qg_fill_Xqual(grid, nodes);
    qsort(node_order, grid->NN, sizeof(int), _comp_node_order);
    SQ=IQ=minQ;

    if(minQ>QGRID_SMOOTH_THRESHOLD)
        goto done;

    memcpy(saved_nodes, nodes, grid->NN*sizeof(node_t));

    /* Turn on smooting machinary */
    SMinitSmoothing(0, NULL, 3,
        OPTMS_DEFAULT, /* technique */
        OPTMS_DEFAULT, /* FunctionID */
        OPTMS_DEFAULT, /* AcceptFunction */
        &smooth_data);

    SMsetUntangleTechnique(smooth_data, OPTMS_COMBINED_UNTANGLING);
    while(minQ<SMALLF)
    {
        for(i=0; i<grid->NN; i++)
        {
            int **lTnds, lNN, lNEL;
            double **lCoords, *lFree;

            nid=node_order[i];
            if(can_move[nid]<0)
                continue;
            /* if(nqual[i]>QGRID_UNTANGLE_THRESHOLD)
                continue; */
            qg_local_connect(grid, nodes, nid,
                &lFree, &lNN, &lNEL, &lCoords, &lTnds);
            SMuntangle(lNN, lNEL, lFree, lCoords, lTnds, smooth_data);
            for(j=0; j<3; j++)
                nodes[nid].coords[j]=(double)lFree[j];
        }
        qg_fill_Xqual(grid, nodes);
        qsort(node_order, grid->NN, sizeof(int), _comp_node_order);
        if(++pass>max_passes)
        {
            warning_msg("max untangling passes exceeded.\n");
            break;
        }
    }

    if(pass)
        PrintLog("    QGrid: untangling passes %u\n", pass);

    /* smart Laplacian pass */
    SMsetSmoothTechnique(smooth_data, OPTMS_SMART_LAPLACIAN_ONLY);
    for(i=0; i<grid->NN; i++)
    {
        int **lTnds, lNN, lNEL;
        double **lCoords, *lFree;

        nid=node_order[i];
        if(can_move[nid]<0)
            continue;
        /* if(nqual[i]>QGRID_SMOOTH_THRESHOLD)
            continue; */
        qg_local_connect(grid, nodes, nid,
            &lFree, &lNN, &lNEL, &lCoords, &lTnds);
        SMsmooth(lNN, lNEL, lFree, lCoords, lTnds, smooth_data);
        for(j=0; j<3; j++)
            nodes[nid].coords[j]=(double)lFree[j];
    }

    SMsetSmoothTechnique(smooth_data, OPTMS_OPTIMIZATION_ONLY);
    /* Optimization based smoothing passes */
    for(pass=1; pass<max_passes; pass++)
    {
        for(i=0; i<grid->NN; i++)
        {
            int **lTnds, lNN, lNEL;
            double **lCoords, *lFree;

            nid=node_order[i];
            if(can_move[nid]<0)
                continue;
            /* if(nqual[i]>QGRID_SMOOTH_THRESHOLD)
                continue; */
            qg_local_connect(grid, nodes, nid,
                &lFree, &lNN, &lNEL, &lCoords, &lTnds);
            SMsmooth(lNN, lNEL, lFree, lCoords, lTnds, smooth_data);
            for(j=0; j<3; j++)
                nodes[nid].coords[j]=(double)lFree[j];
        }
        qg_fill_Xqual(grid, nodes);
        qsort(node_order, grid->NN, sizeof(int), _comp_node_order);
        if(pass<min_passes)
            continue;
        if((minQ<SQ))
        {
            pass--; /* the last pass failed, so we don't count it */
            memcpy(nodes, saved_nodes, grid->NN*sizeof(node_t));
            break;
        }
        if(fabs(minQ-SQ)<0.0001)
            break;
        memcpy(saved_nodes, nodes, grid->NN*sizeof(node_t));
        SQ=minQ;
    }

    /* release memory in qg_local_connect */
    qg_local_connect(grid, nodes, grid->NN, NULL, NULL, NULL, NULL, NULL);
    /* switch off smoothing */
    SMfinalizeSmoothing(smooth_data);

done:
    tm=MPI_Wtime()-tm;
    PrintLog("    QGrid: iQ=%7.5f, SQ=%7.5f,  passes=%2u,  CPU time=%-.3fsec.\n",
        (double)IQ, (double)SQ, pass, tm);

    return;
}

static int _comp_node_order(const void *A, const void *B)
/* return -1 if A is to go before B ... */
{
    double qa=nqual[*(int *)A];
    double qb=nqual[*(int *)B];
    return fabs(qa-qb)<SMALLF?0:qa<qb?-1:1;
}

static void qg_fill_Xqual(grid3d_t *grid, node_t *nodes)
{
    double Q;
    int i, j;
    elem_t *T;
    for(i=0; i<grid->NN; i++)
        nqual[i]=1e99;
    minQ=1e99;
    for(T=grid->elems, i=0; i<grid->NEL; i++, T++)
    {
        if((Q=ElemQuality(nodes, T->nodes))<minQ)
            minQ=Q;
        for(j=0; j<4; j++)
            if(Q<nqual[T->nodes[j]])
                nqual[T->nodes[j]]=Q;
    }
}

static void qg_local_connect(grid3d_t *grid, node_t *nodes, int nid,
    double **lFree, int *lNN, int *lNEL, double ***lCoords, int ***lConn)
{
    int i, j;
    node_inv_t *ninv;

    static double P[3], **nP;
    static int **T;

    if(nP)
    {
        free(*nP);
        free(nP);
        nP=NULL;
    }
    if(T)
    {
        free(*T);
        free(T);
        T=NULL;
    }
    if(nid>=grid->NN)
        return;

    ninv=grid->inverse->ninv+nid;
    nP=malloc(ninv->NN*sizeof(double*));
    T=malloc(ninv->NEL*sizeof(int*));
    if(nP==NULL||T==NULL)
        error_msg("g3dqual.c: Cannot allocate memory.\n");
    *nP=malloc(ninv->NN*3*sizeof(double));
    *T=malloc(ninv->NEL*3*sizeof(int));
    if(*nP==NULL|| *T==NULL)
        error_msg("g3dqual.c: Cannot allocate memory.\n");
    for(i=1; i<ninv->NN; i++)
        nP[i]=nP[i-1]+3;
    for(i=1; i<ninv->NEL; i++)
        T[i]=T[i-1]+3;

    for(i=0; i<3; i++)
        P[i]=(double)nodes[nid].coords[i];
    for(i=0; i<ninv->NN; i++)
        for(j=0; j<3; j++)
            nP[i][j]=(double)nodes[ninv->nids[i]].coords[j];
    for(i=0; i<ninv->NEL; i++)
    {
        const int _ix[4][3]={
            {1, 2, 3},
            {0, 3, 2},
            {3, 0, 1},
            {2, 1, 0}
        };
        int elNds[4];
        int k, fv=4, *egNds;
        egNds=grid->elems[ninv->Tids[i]].nodes;
        for(j=0; j<4; j++)
        {
            if(egNds[j]==nid)
                fv=j;
            else
                for(k=0; k<ninv->NN; k++)
                    if(ninv->nids[k]==egNds[j])
                    {
                        elNds[j]=(int)k;
                        break;
                    }
        }
        if(fv>3)
            error_msg("g3dqual.c: Problem in inverse connectivity table.\n");
        for(k=0; k<3; k++)
            T[i][k]=elNds[_ix[fv][k]];
    }

    if(lFree) *lFree=P;
    if(lCoords) *lCoords=nP;
    if(lConn) *lConn=T;
    if(lNN) *lNN=ninv->NN;
    if(lNEL) *lNEL=ninv->NEL;
}

void init_g3dqual(grid3d_t *grid)
{
    done_g3dqual();
    saved_nodes=malloc(grid->NN*sizeof(node_t));
    Tqual=malloc(grid->NEL*sizeof(double));
    nqual=malloc(grid->NN*sizeof(double));
    node_order=malloc(grid->NN*sizeof(int));
    if(Tqual==NULL||nqual==NULL||node_order==NULL||saved_nodes==NULL)
        error_msg("g3dqual.c: Cannot allocate memory.\n");
    MakeInverse(grid);

}

void done_g3dqual(void)
{
    if(Tqual)
    {
        free(Tqual);
        Tqual=NULL;
    }
    if(nqual)
    {
        free(nqual);
        nqual=NULL;
    }
    if(node_order)
    {
        free(node_order);
        node_order=NULL;
    }
    if(saved_nodes)
    {
        free(saved_nodes);
        saved_nodes=NULL;
    }

}

#include <sys/types.h>
#include <sys/wait.h>
#include <unistd.h>
#include <errno.h>

void fork_QGrid(grid3d_t *grid, node_t *nodes, int *can_move)
{
    ssize_t ignore;
    int pfids[2], status, ret;
    pid_t pid;
    int i;

    if(pipe(pfids)==-1)
    {
        PrintLog("pipe failed: %s\n", strerror(errno));
        goto fail;
    }

    pid=fork();
    switch(pid)
    {
        case -1 :
            PrintLog("fork failed: %s\n", strerror(errno));
            close(pfids[0]);
            close(pfids[1]);
            goto fail;
        case 0:
            QGrid(grid, nodes, can_move);
            for(i=0; i<grid->NN; i++)
                ignore=write(pfids[1], nodes+i, sizeof(node_t));
            _exit(0);
        default:
            for(i=0; i<grid->NN; i++)
            {
                ret=read(pfids[0], nodes+i, sizeof(node_t));
                if(ret!=sizeof(node_t))
                    goto fail;
            }
            ret=-1;
            ret=wait(&status);
            if(ret!=pid)
                warning_msg("%s: wait returned errno = %i %s.\n", __func__,
                errno, strerror(errno));

            close(pfids[0]);
            close(pfids[1]);
            return;
    }

fail:
    QGrid(grid, nodes, can_move);
}


